/*
 *  utils.cpp
 *  bgsubtraction
 *
 *  Created by a1gucis on 3/17/09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "utils.h"

float Utils::min(float r, float g, float b) {
	float min = r;
	if (g < min)
		min = g;
	if (b < min)
		min = b;
	return min;
}

float Utils::max(float r, float g, float b) {
	float max = r;
	if (g > max)
		max = g;
	if (b > max)
		max = b;
	return max;
}

void Utils::ToMatrix(IplImage *image, CvMat *matrix) {
	int step = image->widthStep;
	int channels = image->nChannels;
	uchar* data = (uchar *)image->imageData;
	for(int k=0,j=0; j<image->height; j++) { 
        for(int i=0; i<image->width; i++) { 
			if (channels == 1)
				cvSet1D(matrix,k,cvScalar(data[j*step+i*channels])); // Set element functions for CvMat or IplImage: void cvSet1D( CvArr* arr, int idx0, CvScalar value );
			else  {
				cvSet1D(matrix,k,cvScalar(data[j*step+i*channels+0], data[j*step+i*channels+1], data[j*step+i*channels+2]));
			}
			k++; 
        } 
	} 
	
}

void Utils::ToDMatrix(IplImage *image, CvMat *matrix) {
	cout<<"ToDMAtrix"<<endl;
	int channels = image->nChannels;
	double* data = (double *)image->imageData;
	for(int k=0,i=0; i<image->height; i++) { 
		for(int j=0; j<image->width; j++) { 
			if (channels == 1)
				cvSet1D(matrix,k,cvScalar(*data++)); // Set element functions for CvMat or IplImage: void cvSet1D( CvArr* arr, int idx0, CvScalar value );
			else {
				float chan1 = ((float *)(image->imageData + i*image->widthStep))[j*image->nChannels+0];
				float chan2 = ((float *)(image->imageData + i*image->widthStep))[j*image->nChannels+1];
				float chan3 = ((float *)(image->imageData + i*image->widthStep))[j*image->nChannels+2];
				float value = chan1 * W_CH1 + chan2 * W_CH2 + chan3 * W_CH3;
				//cvSetReal1D(matrix, k, value);
				cvSet1D(matrix, k, cvScalar(chan1 * W_CH1, chan2 * W_CH2, chan3 * W_CH3));
			}
			k++; 
		} 
	}
}

void Utils::ToMatrix(IplImage *image, CvMat *matrix, int channel1Pos) {
	int step = image->widthStep;
	int channels = image->nChannels;
	uchar* data = (uchar *)image->imageData;
	for(int k=0,j=0; j<image->height; j++) { 
        for(int i=0; i<image->width; i++) { 
			cvSet1D(matrix,k,cvScalar(data[j*step+i*channels+channel1Pos]));
			k++; 
        } 
	} 
	
}
void Utils::ToMatrix(IplImage *image, CvMat *matrix, int channel1Pos, int channel2Pos) {
	int step = image->widthStep;
	int channels = image->nChannels;
	uchar* data = (uchar *)image->imageData;
	for(int k=0,j=0; j<image->height; j++) { 
        for(int i=0; i<image->width; i++) { 
			cvSet1D(matrix,k,cvScalar(data[j*step+i*channels+channel1Pos], data[j*step+i*channels+channel2Pos]));
			k++; 
        } 
	} 
	
}

void Utils::ToNormalImg(IplImage *image, int numOfClusters) {
	int offset = 255 / numOfClusters;
	int step = image->widthStep;
	uchar *data = (uchar *)image->imageData;
	for (int i=0;i<image->height;i++) {
		for (int j=0;j<image->width;j++) {
			data[i*step+j] = (data[i*step+j]+1) * offset;
		}
	}
}

float Utils::MatrixSum(CvMat *mat) {
	float sum = 0;
	for (int i=0;i<mat->rows;i++) {
		float *ptr = (float*)(mat->data.ptr + i * mat->step); 
		for (int j=0;j<mat->cols;j++) {
			sum += *ptr++;
		}
	}
	return sum;
}
void Utils::PrintMat(const CvMat *matrix) {
	int total = 0;
	for (int i=0;i<matrix->rows;i++) {
		for (int j=0;j<matrix->cols;j++) {
			double c = (double)cvmGet(matrix,i,j);
			//if (c > 0.1) {
				//total++;
				cout<<c<<"\t";
			//}
			total++;
		}
		cout<<endl;
	}
	cout<<"total elements in matrix: "<<total<<endl;
	
}

void Utils::PrintDImg(IplImage *image) {
	int total = 0;
	double *data = (double *)image->imageData;
	cout<<image->height<<" "<<image->width<<endl;
	for (int i=0;i<image->height;i++) {
		for (int j=0;j<image->width;j++) {
			double c1 = *data++;
			double c2 = *data++;
			double c3 = *data++;
			cout<<"i: "<<i<<" j:"<<j<<" | "<<c1<<" "<<c2<<" "<<c3<<endl;
			//if ((c1 > 0.1) || (c2 > 0.1) || (c3 > 0.1))
			//	total++;
			total += 3;
			/*cout<<"i:"<<i<<" j:"<<j<<"|"<<data[i*image->widthStep+j*image->nChannels+0]<<" ";
			cout<<data[i*image->widthStep+j*image->nChannels+1]<<" ";
			cout<<data[i*image->widthStep+j*image->nChannels+2]<<endl;	*/		
		}
	}
	cout<<"total number of elements in image: "<<total<<endl;
}

void Utils::FillImg(IplImage *image, int color) {
	uchar *data = (uchar *)image->imageData;
	for (int i=0;i<image->height;i++) {
		for (int j=0;j<image->width;j++) {
			data[i*image->widthStep+j*image->nChannels] = color;
		}
	}
}

void Utils::AppendImg(IplImage *src, IplImage *dst, int imgNumber) {
	uchar *dstData = (uchar *)dst->imageData;
	uchar *srcData = (uchar *)src->imageData;
	dstData += imgNumber*((src->height-1)*dst->widthStep+src->width*dst->nChannels);
	for (int i=0;i<src->height;i++) {
		for (int j=0;j<src->width;j++) {
			dstData[i*dst->widthStep+j*dst->nChannels+0] = srcData[i*src->widthStep+j*src->nChannels+0];
			dstData[i*dst->widthStep+j*dst->nChannels+1] = srcData[i*src->widthStep+j*src->nChannels+1];
			dstData[i*dst->widthStep+j*dst->nChannels+2] = srcData[i*src->widthStep+j*src->nChannels+2];
		}
	}
}

void Utils::ToHSIImg(IplImage *src, IplImage *dst) {
	uchar *srcData = (uchar *)src->imageData;
	for (int i=0;i<src->height;i++) {
		for (int j=0;j<src->width;j++) {
			float r = (float)srcData[i*src->widthStep+j*src->nChannels+0];
			float g = (float)srcData[i*src->widthStep+j*src->nChannels+1];
			float b = (float)srcData[i*src->widthStep+j*src->nChannels+2];
			
			float min = r;
			if (g < min)
				min = g;
			if (b < min)
				min = b;
			
			float chI = (r+g+b)/3;
			float chS;
			float chH;

			if (chI == 0)
				chS = 1;
			else
				chS = 1 - min/chI;
			if (chS == 0.0)
				chH = 0.0;
			else {
				chH = ((r-g)+(r-b))/2.0;
				float sqrtAns = sqrt((r-g)*(r-g)+(r-b)*(g-b));
				if (sqrtAns == 0)
					chH = 0.0;
				else
					chH = chH/sqrtAns;
				chH = acos(chH);
				if (b > g)
					chH = 2 * M_PI - chH;
				chH = chH/(2*M_PI);
			}
			((float *)(dst->imageData + i*dst->widthStep))[j*dst->nChannels+0] = chH;
			((float *)(dst->imageData + i*dst->widthStep))[j*dst->nChannels+1] = chS;
			((float *)(dst->imageData + i*dst->widthStep))[j*dst->nChannels+2] = chI/255.0;
			/*I =(R+G+B)/3; 
			S = 1 - min/I; 
			if (S == 0.0) 
				H = 0; 
			else 
				H = ((R-G)+(R-B))/2.0; 
			H = H/sqrt((R-G)*(R-G) + (R-B)*(G-B)); 
			H = acos(H); 
			if (B > G) 
				
				H = 2*PI - H; 
			end 
			H = H/(2*PI); 
			end 
			hsi_h(i,j)=H;hsi_s(i,j)=S;hsi_i(i,j)=I/255; */
			
		}
	}
}

void Utils::ToWeightedImg(IplImage *src, IplImage *dst) {
	uchar *srcData = (uchar *)src->imageData;
	uchar *dstData = (uchar *)dst->imageData;
	
	for (int i=0;i<src->height;i++) {
		for (int j=0;j<src->width;j++) {
			float chan1 = (float)srcData[i*src->widthStep+j*src->nChannels+0];
			float chan2 = (float)srcData[i*src->widthStep+j*src->nChannels+1];
			float chan3 = (float)srcData[i*src->widthStep+j*src->nChannels+2];
			
			dstData[i*dst->widthStep+j*dst->nChannels+0] = (char)round(chan1 * W_CH1);
			dstData[i*dst->widthStep+j*dst->nChannels+1] = (char)round(chan2 * W_CH2);
			dstData[i*dst->widthStep+j*dst->nChannels+2] = (char)round(chan3 * W_CH3);
		}
	}
}

void Utils::ToWeightedImg32F(IplImage *src, IplImage *dst) {
	for (int i=0;i<src->height;i++) {
		for (int j=0;j<src->width;j++) {
			float chan1 = ((float *)(src->imageData + i*src->widthStep))[j*src->nChannels + 0];
			float chan2 = ((float *)(src->imageData + i*src->widthStep))[j*src->nChannels + 1];
			float chan3 = ((float *)(src->imageData + i*src->widthStep))[j*src->nChannels + 2];
			
			((float *)(dst->imageData + i*dst->widthStep))[j*dst->nChannels + 0] = chan1 * W_CH1;
			((float *)(dst->imageData + i*dst->widthStep))[j*dst->nChannels + 1] = chan2 * W_CH2;
			((float *)(dst->imageData + i*dst->widthStep))[j*dst->nChannels + 2] = chan3 * W_CH3;
			
		}
	}
}

void Utils::CheckForNans(IplImage *image) {
	bool nansFound = false;
	for (int i=0;i<image->height;i++) {
		for (int j=0;j<image->width;j++) {
			float chan1 = ((float *)(image->imageData + i*image->widthStep))[j*image->nChannels + 0];
			float chan2 = ((float *)(image->imageData + i*image->widthStep))[j*image->nChannels + 1];
			float chan3 = ((float *)(image->imageData + i*image->widthStep))[j*image->nChannels + 2];
			if ((isnan(chan1)) || (isnan(chan2)) || (isnan(chan3)))
				nansFound = true;
		}
	}
	if (nansFound)
		cout<<"NANS FOUND"<<endl;
}